#include "MatchingEstimator.h"


MatchingEstimator::MatchingEstimator(void)
{
}


MatchingEstimator::~MatchingEstimator(void)
{
}

void MatchingEstimator::createObserveSkin()
{
	if (ObserveDepth.data==NULL){
		printf("Observe Depth is null");
		return;
	}
	cv::Mat skinPreview = cv::Mat::zeros(mHEIGHT,mWIDTH,CV_8UC1);
	for (int i=0;i<mHEIGHT;i++){
		for (int j=0;j<mWIDTH;j++){
			ObserveSkin[i][j]=false;
			/* |Od - Rd| */
			unsigned short Od = ObserveDepth.at<unsigned short>(i,j);
			if (Od<0xffff){
				ObserveSkin[i][j]=true;
				skinPreview.at<uchar>(i,j)=255;
			}
		}
	}

	if (SHOW_DEBUG)
	{
		imshow("Skin Preview",skinPreview);
		cv::waitKey();
	}
}

void MatchingEstimator::createRenderedMap()
{
	if (ObserveDepth.data==NULL || renderedDepth.data==NULL ){
		printf("data is null");
		return;
	}

	//@note: that OPENCV doesn't support binary Matrix
	//@using renderedMatched for drawing Rm
	cv::Mat renderedMatched = cv::Mat::ones(mHEIGHT,mWIDTH,CV_8UC1);
	for (int i=0;i<mHEIGHT;i++){
		for (int j=0;j<mWIDTH;j++){
			rMatch[i][j]=false;
			/* |Od - Rd| */
			uint16_t Od = ObserveDepth.at<uint16_t>(i,j);
			uint16_t Rd = renderedDepth.at<uint16_t>(i,j);
			uint16_t sub = abs(Od-Rd);
			if (sub<Dmax && Rd < 0xffff){
				rMatch[i][j]=true;
				renderedMatched.at<uint8_t>(i,j)=255;
			}
		}
	}

	if (SHOW_DEBUG)
	{
		imshow("R-matched",renderedMatched);
		cv::waitKey();
	}

}

void MatchingEstimator::getD(){
	double sum_1=0; // depth evaluate
	double sum_2=0; // Os v rm
	double sum_3=0; // Os & rm
	for (int i=0;i<mHEIGHT;i++){
		for (int j=0;j<mWIDTH;j++){
			uint16_t Od = ObserveDepth.at<uint16_t>(i,j);
			uint16_t Rd = renderedDepth.at<uint16_t>(i,j);
			int gain_1 = DMAX;
			if (Rd<0xffff || Od<0xffff)
			{
				if (abs(Od-Rd) < DMAX)	gain_1 = abs(Od-Rd);
				assert(gain_1<=DMAX);
				sum_1+= (gain_1/10);// chia 10 de convert ve milimet
			}
			


			sum_2+= ObserveSkin[i][j] || rMatch[i][j];
			sum_3+=	ObserveSkin[i][j] && rMatch[i][j];
		}
	}
	printf("sum_1 = %f \n",sum_1);
	printf("sum_2 = %f \n",sum_2);
	printf("sum_3 = %f \n",sum_3);

	double result = sum_1/(sum_2+0.000001) + LAMDA*(1- 2*sum_3/(sum_2+sum_3));

	printf("D = %f \n",result);
	cv::waitKey();
}